#include <cv.h>
#include <sdopencv/hog.hpp>

namespace sdopencv
{
    // octagon with diagonals x=2y, -x=2y, 2x=y, 2x=-y
    unsigned char get_orientation2(short x, short y)
    {
        if(!x && !y) return 0;
        if(y<<1 >= -x) // 1, 2, 3, 4
            if(x<<1 >= y) // 1, 2
                if(y<<1 <= x) // 1
                    return 1;
                else // 2
                    return 2;
            else // 3, 4
                if(x<<1 >= -y) // 3
                    return 3;
                else // 4
                    return 4;
        else // 5, 6, 7, 8
            if(x<<1 <= y) // 5, 6
                if(y<<1 >= x) // 5
                    return 5;
                else // 6
                    return 6;
            else // 7, 8
                if(x<<1 <= -y) // 7
                    return 7;
                else // 8
                    return 8;
    }


    // octagon with diagonals x=0, y=0, x=y, x=-y
    unsigned char get_orientation(short x, short y)
    {
        if(y > 0) // 1, 2, 3, 4
        {
            if(x > 0) return (y >= x)? 2: 1;
            else return (y > -x)? 3: 4;
        }

        if(y < 0) // 5, 6, 7, 8
        {
            if(x < 0) return (x < y)? 5: 6;
            else return (x >= -y)? 8: 7;
        }

        // 0, 1, 5
        if(x > 0) return 1;
        if(x < 0) return 5;
        return 0;
    }


    // compute the oriented gradient image of an image
    // src = 8U 1 channel
    // tmp1, tmp2 = 16S 1 channels
    // dst = 8U 1 channel
    // for each pixel (x,y), dst(x,y) = index (from 0 to 8) of the orientation of the gradient
    void cvOrientedGradient(IplImage *src, IplImage *dst, IplImage *tmp1, IplImage *tmp2)
    {
        cvSobel(src, tmp1, 1, 0); // x derivative
        cvSobel(src, tmp2, 0, 1); // y derivative

        CvSize sz = cvGetSize(src);
        int w = sz.width, h = sz.height;
        int x, y;
        short *dx, *dy;
        unsigned char *dz;

        for(y = 0; y < h; ++y)
        {
            dx = (short *)&tmp1->imageData[y*tmp1->widthStep];
            dy = (short *)&tmp2->imageData[y*tmp2->widthStep];
            dz = (unsigned char *)&dst->imageData[y*dst->widthStep];

            for(x = 0; x < w; ++x)
                dz[x] = get_orientation(dx[x], dy[x]);
        }
    }
}

